/* Pendulum1 a pendulum model using the Euler-Richardson Algorithm by Ryan Haynes */

import java.text.*;

class Pendulum1{
	
	DecimalFormat myDec = new DecimalFormat("0.000");
	//initialize instance variables
	double damp = 0.5;
	double driveAmp = 1.2;
	double driveFreq = 2.0/3.0;
	double dt = 0.01;
	
	Pendulum1(double degrees){
		swing(degrees * Math.PI/180);  //convert degrees to radians when swing is called
	}
	
	public void swing(double theta) {

		//Initialize new plot
		Plot swingPlot = new Plot("Theta vs Time",0,70,1,-5*Math.PI,5*Math.PI,Math.PI/4);

		//Initialize variables
		double alpha;
		double omega = 0;
		double alphaMid;
		double thetaMid;
		double lastTheta;
		double lastZeroTime=0;
		double zeroTime;
		double omegaMid;
		double totalEnergy;
		double period;
		System.out.print("The beginning energy is ");
		System.out.println(1-Math.cos(theta));
		
		//Start Euler Richardson algorithm
		for (double t=0;t<=100;t+=dt) {
				
			alpha = torque(theta,omega,t);
			thetaMid = theta + omega * 0.5 * dt;
			omegaMid = omega + alpha * 0.5 * dt;
			alphaMid = torque(thetaMid,omegaMid,t + dt/2);
			lastTheta = theta;
			theta += omegaMid * dt;
			omega += alphaMid * dt;

			//Calculate the period of swing
			
			if (theta * lastTheta <=0) {
				zeroTime = t + dt - theta/omega;
				System.out.print("The period is ");
				System.out.println(myDec.format(2*(zeroTime-lastZeroTime)));
				lastZeroTime = zeroTime;
			}
			swingPlot.addPoint(t,theta);
		}
		totalEnergy = 1-Math.cos(theta) + .5*omega*omega;
		System.out.print("The total energy is ");
		System.out.println(totalEnergy);
	}
	
	//function to calculate acceleration
	double torque(double theta, double omega, double t) {
		return -Math.sin(theta) -damp*omega + driveAmp*Math.sin(driveFreq*t);
	}
	
	public static void main(String[] args) {
		double degrees = 20.0;

		try {degrees = Double.parseDouble(args[0]);}
			catch (ArrayIndexOutOfBoundsException e) {}
			catch (NumberFormatException e) {}
			
		new Pendulum1(degrees);

	}
}
